#ifndef __UBT_COMMUNICATION_H__
#define __UBT_COMMUNICATION_H__

#include <memory>
#include <thread>
#include <jsoncpp/json/json.h>
#include <cstdlib>
#include <ubt_mqtt/node.h> 



#include <geometry_msgs/msg/pose2_d.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/polygon_stamped.hpp>
#include <geometry_msgs/msg/point32.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/int64.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <std_msgs/msg/int16_multi_array.hpp>
#include <std_msgs/msg/float32.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <map>
#include <deque>
#include <tf2_ros/transform_listener.h>
#include "httplib.h"

#include <utility>
#include "map_manager.h"
#include "mqtt_client.h"
#include "ubt_yaml_config.h"
#include "config.h"
#include "message_manager.h"
#include "ubt_utils.h"
#include "task_manager.h"
#include "ubt_interface/msg/battery_data.hpp"
#include "ubt_interface/msg/wifi_info.hpp"

#include <std_srvs/srv/set_bool.hpp>
#include <std_srvs/srv/trigger.hpp>

namespace ubt {

/**
 * 状态机的状态处理模块* 
 */
struct u_state_status_info_t
{
    u_state_status_info_t()
    {
        _cur_state = " ";
        _last_state = " ";
        _state_start_time = 0;
    }

    ~u_state_status_info_t()
    {
    }

    void UpdateState(const std::string& state, const std::function<void(const std::string &state)> &callback = nullptr)
    {
        _cur_state = state;
        if (_cur_state != _last_state)
        {
            RCLCPP_INFO_STREAM(node->get_logger(),"last state: " << _last_state << ", cur state: " << _cur_state);

            _last_state = _cur_state;
            _state_start_time = ubt_utils::getTimeMilliseconds();
            if (callback)
            {
                callback(_cur_state);
            }
        }
        else
        {
            _state_duration = ubt_utils::getTimeMilliseconds() - _state_start_time;
        }
    }

    const std::string &getCurState()
    {
        return _cur_state;
    }

    uint64_t getStateDuration()
    {
        return _state_duration;
    }

private:
    std::string _cur_state;
    std::string _last_state;
    uint64_t _state_start_time; // ms
    uint64_t _state_duration;   // 当前状态持续时间

};

/**
 * mqtt 消息发布管理
 */
struct messages_publish_manager
{
    bool need_send(const uint32_t rate)
    {
        int64_t cur_time = ubt_utils::getTimeMilliseconds();
        if (cur_time - _last_send_time >= rate)
        {
            _last_send_time = cur_time;
            return true;
        }

        return false;
    }
private:
    int64_t _last_send_time = 0; // 上一次上报时间, ms
};

enum E_PUBLISH_MANAGER
{
    E_HEART_BEAT_MANAGER = 0,       // 心跳
    E_STATE_STATUS_MANAGER,         // 状态机状态
    E_REAL_SPEED_MESSAGE,           // 实时速度信息
    E_MAP_TASK_INFO_MESSAGE,        // 上报地图 / 任务信息 (1500 ms)
    E_SEND_AGV_VERSION,             // 上报agv版本信息 (10000 ms)
    E_CHECK_MQTT_CONNECT_STATUS,    // 检测 mqtt 连接状态
    E_BATTERY_DATA_MESSAGE,         // 上报电池信息 (1000 ms)
    E_WIFI_INFO_MESSAGE,            // 上报wifi信息 (1000 ms)
    E_PUBLISH_MANAGER_MAX
};

class CUbtCommunicationNode
{
private:
    std::shared_ptr<CMapManager> _pMapStr;
    std::unique_ptr<CMqttClient> _pMqttStr;
    std::unique_ptr<CMessageManager> _pMessageStr;
    std::unique_ptr<CTaskManager> _pTaskManager;

    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pubUbtUpdate;       // 在线升级 (topic: "ubt_update")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubMapUpdate;       // 通知地图更新  (topic: "ui_map_update")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubFixedPathTask;   // 固定路径更新  (topic: "ui_fixedpath_task")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubRealTaskLimit;   // 交管限制     (topic: "task_limit_index")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubCmdState;        // 控制子任务   (topic: "ui_state")
    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr _pubInitLocation;    // 发送初始定位  (topic: "initialpose_uslam"(uslam) / "initialpose"(amr))
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubConfirm;         // 执行度确认   (topic: "pose_confirm")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubCtrlCarto;       // 建图控制     (topic: "ui_stop_carto")
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr _pubCtrlGmapping;    // 建图控制     (topic: "task_gammping_control" (uslam))
    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pubMqttStatus;      // mqtt链接状态 (topic: "mqtt_status")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubAgvTaskId;       // 当前任务id (topic: "agv_task_id")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubAgvTaskName;     // 当前任务名称 (topic: "agv_task_name")
    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pubCleanRecTask;    // 清除恢复任务 （topic: "clean_recover_task")
    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pubCharacteristic;  // 特征扫描 （topic: "ui_characteristic")
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr _pubChargeCtrl;      // 打开充电回路 （topic: "charge_ctrl")
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr _pubLiftSpeedAndDistance; //发布f1200s货叉控制 （topic: "lift_speed_and_distance")
    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr _pubLiftSpeed;       // 发布u1000顶升控制 （topic: "lift_speed")
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr _pubTractionLatchCtrl;// 发布牵引销控制 （topic: "traction_latch_ctrl")
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr _pubEventReset;      // 复位 （topic: "event_reset")
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr _pubLiftingJackCtrl; // 驱动轮控制 (topic: "lifting_jack_ctrl")
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr _pubTwist;           // 通过wifi发送速度 （topic: "ui_cmd_vel")
    rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr _pubUslamMappingCtrl;// USLAM 建图控制 （topic: "mappingstate")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubMutiMapList;     // 状态机存下所有点数据 （topic: "muti_map_list")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubInitFindRfid;    // ui发送寻找rfid定位（topic: "init_find_rfid")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubUiConfigUpdate;  // 通知其他模块ui参数已经更新 (topic: "ui_config_update")
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pubAgvBattery;      // 电池信息 (topic: "agv_battery")
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr _pubRemoteEmergency;      // 远程软急停 (topic: "remote_emergency")
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr _pubLiftActionPause;      // 转发倒退取货取消任务 (topic: "lift_action_pause")

    rclcpp::Subscription<std_msgs::msg::Int16MultiArray>::SharedPtr _subAgvStatus;      // agv状态信息  (topic: "/agv/status")
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _subAgvDiagWarn;    // agv告警信息  (topic: "/agv/diag_warn")
    rclcpp::Subscription<geometry_msgs::msg::Pose2D>::SharedPtr _subCurrentPose;    // 当前坐标信息 (topic: "current_pose")
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _subStateStatus;    // 状态机状态　(topic: "ubt_state_status")
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr _subCpuRatio;       // cpu占用率  (topic: "cpu_ratio")
    // rclcpp::Subscription _subBatterySoc;     // 电池电量信息 (topic: "BatteryData");
    rclcpp::Subscription<ubt_interface::msg::BatteryData>::SharedPtr _subBatteryData;    // 订阅电池电量   （topic: "BatteryData")
    rclcpp::Subscription<ubt_interface::msg::WifiInfo>::SharedPtr _subWifiInfo;       // 订阅wifi信息   （topic: "wifi_info")
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _subTaskStaus;      // 任务状态   (topic:  "task_status")
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr _subRealSpeed;      // 电机实时速度 (tipoc: "real_speed")
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _subResetDispath;   // 急停/防撞条后取消任务反馈 (topic: "reset_dispatch")
    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr _subRobotWorkInfo;  // robot运行信息(里程和运行时间) (topic: "robot_work_info")
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr _subMapConfidence;  // 订阅地图置信度 (topic: "map_confidence")
    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr _subMapData;        // 订阅实时建图数据 (topic: "slam_map"(uslam) / "real_time_image"(amr))
    rclcpp::Subscription<std_msgs::msg::Int16MultiArray>::SharedPtr _subRealRouteInfo;  // 当前路径id    (topic: "real_route_info")
    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr _subSlamMapFinish;  // uslam建图结束  (topic: "slam_map_finish")
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr _subLaserPoints;    // 订阅雷达激光点  (topic: "r2000_node/scan")
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr _subEventLimit;     // 订阅顶升状态  (topic: "lidar_scan")
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _subAgvLastTask;    // 上一次任务记录  (topic: "agv_last_task")

    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr enableLearnQrcodePositionClient = node->create_client<std_srvs::srv::SetBool>("enable_learn_qrcode_position");
    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr reloadMapClient = node->create_client<std_srvs::srv::Trigger>("reload_map");
    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr reloadQrcodeClient = node->create_client<std_srvs::srv::SetBool>("reload_qrcode_position");


    struct mqtt_paramer_t _MqttParamer;
    struct robot_version_info_t _RobotVersion;

    std::string _Agv_type;
    std::string _agv_loc_algs;

    u_state_status_info_t _StateInfo;
    messages_publish_manager _PublishMamager[E_PUBLISH_MANAGER_MAX];

    double _CpuRatioData;

    double _mapWidth;
    double _mapHeight;
    double _map_x;
    double _map_y;
    double _TotalOdom; // 机器人运行的总里程
    double _TotalTime; //　机器人运行的总时间

    std::string _defaultAgvAlgs;
    std::string _TaskPublishData;
    std::string _ConfigVersion; // 配置项版本    
    std::string _mapRequestId; // 地图请求id
    std::string _pubMappingSuccessTopic; // 建图成功发布topic
    std::deque<Json::Value> real_task_queue;
    bool _openEventLimit;
    bool _openLidar;
    bool _openTractionLatchState;
    bool _isPubMutiMapList;
    std_msgs::msg::UInt32 _eventLimit;

    //tf
    // tf::TransformListener tf_listener;
    std::vector<double> _laser_to_robot_trans;
    double _laser_to_robot_yaw;

    //获取包的绝对路径
    std::string _graphSlamRos;
    std::string _gmappingRos;
    std::string _pyDemoPath;
    std::string _ubtTrajectoryTrack;
    
    // param
    std::string _mapOriginYaml;
    std::string _mapDataDir;
    std::string _originImage;
    std::string _qrcode_poses_yaml;
    std::string _task_yaml;
    std::string _mapLocalPointYAML;
    std::string _last_task_json = "/home/ubt/ubt_agv_install/cfg/maps/last_task.json"; // 记录具体的任务信息
    std::string _last_task_yaml = "/home/ubt/ubt_agv_install/cfg/maps/last_task.yaml"; // 记录具体的任务信息
    std::string _muti_map_list_json = "/home/ubt/ubt_agv_install/cfg/maps/muti_map_list.json";
    Json::Value _muti_map_list; 
    // std::string qrcode_poses_yaml_path;
    const char* _mappingType = std::getenv("MAPPING_TYPE");

    std::deque<Json::Value> _realTaskDeque; // 存储具体的任务信息
    std::atomic_bool _MappingStatus;     // false: 停止建图 true: 开始建图
    std::atomic_bool _UslamMapFinished;  // uslam 建图是否结束

public:
    CUbtCommunicationNode();
    ~CUbtCommunicationNode();

    bool Initialize();

private:
    void AgvStatusCallback(const std_msgs::msg::Int16MultiArray::SharedPtr msgs);
    void AgvDiagWarnCallback(const std_msgs::msg::String::SharedPtr msgs);
    void CurrentPoseCallback(const geometry_msgs::msg::Pose2D::SharedPtr msgs);
    void StateStatusCallback(const std_msgs::msg::String::SharedPtr msgs);
    void CpuRatioCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msgs);
    void TaskStatusCallback(const std_msgs::msg::String::SharedPtr msgs);
    void RealSpeedCallback(const geometry_msgs::msg::Twist::SharedPtr msgs);
    void ResetDispatchCallback(const std_msgs::msg::String::SharedPtr msgs);
    void RobotWorkInfoCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msgs);
    void MapConfidenceCallback(const std_msgs::msg::Float32::SharedPtr msgs);
    void MapImageCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msgs);
    void RealRouteInfoCallback(const std_msgs::msg::Int16MultiArray::SharedPtr msgs);
    void SlamMapFinishImageCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msgs);
    void LidarLaserPointsCallback(const sensor_msgs::msg::LaserScan::SharedPtr msgs);
    void EventLimitCallback(const std_msgs::msg::UInt32::SharedPtr msgs);
    void OnAgvLastTaskCallback(const std_msgs::msg::String::SharedPtr msgs);
    void OnBatteryDataCallback(const ubt_interface::msg::BatteryData::SharedPtr msg);
    void OnWifiInfoCallback(const ubt_interface::msg::WifiInfo::SharedPtr msg);

    void MqttMessageCallbackAdd();
    Json::Value get_qrcode_poses_by_map_list();

    void OnMappingMessageCallback(const char* data, int len);       // 建图
    void OnHeartbeatMessageCallback(const char* data, int len);     // 心跳
    void OnMissionMessageCallback(const char* data, int len);       // 地图设置
    void OnTaskMessageCallback(const char* data, int len);          // task
    void OnRealTaskMessageCallback(const char* data, int len);      // 实时任务
    void OnRealLimitTaskMessageCallback(const char* data, int len); // 实时限制性任务
    void OnConfigMessageCallback(const char* data, int len);        // 设置配置信息
    void OnRequsetConfigMessageCallback(const char* data, int len); // 获取配置信息
    void OnCommandMessageCallback(const char* data, int len);       // 后台命令
    void OnManualMoveMessageCallback(const char* data, int len);    // 
    void OnInitLocationMessageCallback(const char* data, int len);  // 定位

    void SubscribeResponce(std::string requsetid, bool status, std::string errmmsg);

    void PublishTaskMessage();
    void pubMappingSuccessToWeb(const std::string& topic);
    std::string exec(const char* cmd);


    void UpdateConfigFile(const Json::Value& config);
    void RequestConfigMessage(const Json::Value& root);
    std::pair<bool, std::string> PraseMissionMessage(Json::Value root);
    int  ReadAndSendAgvTask();
    void readMapParam(const std::string& orginImage, const std::string& mapOrginYaml);
    void updateLocalPointFile(const Json::Value& mapPoints);

    bool StartMapping();
    bool StopMapping();

    void MqttThread();
    void LoopThread(void);
};

}

#endif // !__UBT_COMMUNICATION_H__

